89128c71a49943e3f47152420dbcccb14fb70536,src/org/usfirst/frc/team3641/robot/Tracking.java,Tracking,autoTarget,#,47

Before Change


				visionState = Constants.SEND_REQUEST;
			}
			
			if(Math.abs(error) < 1) SmartDashboard.putBoolean("TRACKED", true);
			else SmartDashboard.putBoolean("TRACKED", false);

		}

After Change


			}
			else
			{
				SmartDashboard.putBoolean("TRACKED", false);
				visionState = Constants.TURN_TO_TARGET;
			}

		}
		
		else if(visionState == Constants.WAIT_FOR_STILL)
		{
			newAngle = DriveBase.getDriveDirection();
			if(Math.abs(newAngle-oldAngle) > Constants.MOTION_THRESHOLD)
			{
				DriveBase.driveNormal(0.0, 0.0);
			}
			else
			{
				visionState = Constants.SEND_REQUEST;
			}
			
			oldAngle = newAngle;
		}
		
		else if (visionState == Constants.TURN_TO_TARGET)
		{
			/*
			double driveOutput = -1* PILoop.smoothDrive(DriveBase.getDriveDirection(), target, false);
			}
			*/
			double ActualCurrentHeading = DriveBase.getDriveDirection();
			double error = target - ActualCurrentHeading;
			SmartDashboard.putNumber("Adjusted Heading", ActualCurrentHeading);
			if(error>=180)
			{
				error -= 360;
			}
			else if(error<=-180)
			{
				error += 360;
			}
			if(error>=2 || error <=1 || true)
			{
				if (errorRefresh > Constants.KI_UPPER_LIMIT)
				{
					errorRefresh = Constants.KI_UPPER_LIMIT;
				}
				else if (errorRefresh < Constants.KI_LOWER_LIMIT)
				{
					errorRefresh = Constants.KI_LOWER_LIMIT;
				}
				errorRefresh += error;
				driveOutput = -1 * (((error * Constants.DRIVE_KP) + (errorRefresh * Constants.DRIVE_KI)));
				if (driveOutput > 0)
				{
					driveOutput+= .1;
				}
				else
				{
					driveOutput-= .1;
				}
				if (Math.abs(driveOutput) > .55)
				{
					if (driveOutput < 0)
					{
						driveOutput = -.55;
					}
					
					else
					{
						driveOutput = .55;
					}
				}
				
				DriveBase.driveNormal(0.0, driveOutput);
			}
			
			if(Math.abs(error) < 1)
			{
				SmartDashboard.putBoolean("TRACKED", true);
				DriveBase.driveNormal(0.0, 0.0);
				visionState = Constants.WAIT_FOR_STILL;
				oldAngle = ActualCurrentHeading;